#ifndef _DYNAMIC_TRACKLET_
#define _DYNAMIC_TRACKLET_

// Includes:
// --------
// STL
#include <algorithm>
#include <limits>

// ROS/OpenCV/PCL
#include <opencv2/core/core.hpp>
#include <image_geometry/pinhole_camera_model.h>

// Project
#include "tracklet.h"
#include "interval.h"

// Namespaces:
// ----------
using namespace std;

// ------------------------------------
class DynamicTracklet : public Tracklet
// ------------------------------------
{
	private:
		cv::Rect screenROI;									// A rectangle representing the entire screen
		tf::TransformListener* transformListener; 			// The transform listener used to project/unproject the point
		string fixedFrame;									// The frame id of the fixed frame
		image_geometry::PinholeCameraModel cameraModel;		// The camera model we're using
		double maxMoveM;									// The max distance we antipate someone moving
		ros::Duration timeout;								// The longest we're willing to wait for a transform

		tf::Point ptFixed;									// The guts of the tracklet -- this is the point (in a fixed frame) that the tracklet is "centered" around

	public:
		// -------------------------------------------------------------------------------------------------------------------------------------------------------
		DynamicTracklet(geometry_msgs::PointStamped pt, double maxMoveM, image_geometry::PinholeCameraModel cameraModel, tf::TransformListener* transformListener)
		// -------------------------------------------------------------------------------------------------------------------------------------------------------
		{
			// Initialize the paramaters
			this->transformListener = transformListener;
			this->cameraModel = cameraModel;
			this->screenROI = cv::Rect(0,0, cameraModel.width(), cameraModel.height());
			this->maxMoveM = maxMoveM;

			// Initialize our constants
			this->fixedFrame = string("/map");
			this->timeout = ros::Duration(10.0/30.0); 			// Assumes a camera framerate of ~30fps

			// Update the tracklet with the initial values
			this->update(pt);
		}

		// ------------------------------------------------
		virtual void update(geometry_msgs::PointStamped pt)
		// ------------------------------------------------
		{
			Tracklet::update(pt);

			try
			{
				// Project the stamped point into the fixed frame
				ros::Time acquisition_time = pt.header.stamp;
				tf::StampedTransform transform;
				this->transformListener->waitForTransform(this->fixedFrame, pt.header.frame_id, pt.header.stamp, this->timeout); 
				this->transformListener->lookupTransform(this->fixedFrame, pt.header.frame_id, pt.header.stamp, transform);

				// Do the transformation
				tf::Point ptUnfixed(pt.point.x, pt.point.y, pt.point.z);
				this->ptFixed = transform * ptUnfixed;

			}catch(tf::LookupException& e)
			{
				ROS_WARN("TF | %s", e.what());
			}catch(tf::ExtrapolationException& e)
			{
				ROS_WARN("TF | %s", e.what());
			}catch (tf::TransformException& e)
			{
				ROS_WARN("TF | %s", e.what());
			}	

		}

		// ------------------------------------------------------
		virtual cv::Rect getROI(string& cameraFrame, ros::Time t)
		// ------------------------------------------------------
		{
			try{
				// Lookup the transform from the fixed frame --> camera frame
				tf::StampedTransform transform;
				this->transformListener->waitForTransform(cameraFrame, this->fixedFrame, t, this->timeout); 
				this->transformListener->lookupTransform(cameraFrame, this->fixedFrame, t, transform); 
				
				// Do the transform
				tf::Point ptCamera = transform * this->ptFixed;

				// Project the point to 2D
				cv::Point3d pt3d(ptCamera.x(), ptCamera.y(), ptCamera.z());
				cv::Point2d pt2d;
				this->cameraModel.project3dToPixel(pt3d, pt2d);

				// Create the region of interest
				double dU = this->getDeltaU(this->cameraModel, this->maxMoveM, pt3d.z);
				double dV = this->getDeltaV(this->cameraModel, this->maxMoveM, pt3d.z);
				cv::Rect roi((int)(pt2d.x - dU), (int)(pt2d.y - dV), (int)(2 * dU), (int)(2 * dV));

				// Calculate the intersection of the ROI and the screen
				roi = this->screenROI & roi;
				return roi;

			}catch(tf::LookupException& e)
			{
				ROS_WARN("TF Exception  | %s", e.what());
				return this->screenROI;
			}catch(tf::ExtrapolationException& e){
				ROS_WARN("TF Exception  | %s", e.what());
				return this->screenROI;
			}
		}

		// ----------------------------
		virtual bool contains(double d)
		// ----------------------------
		{
			return true;
		}
};

#endif /* _DYNAMIC_TRACKLET_ */
